#!/usr/bin/env python3

import sys, time
import threading
import binascii
import json
import RPi.GPIO as GPIO
import nnpy

        
class Drive(object):
    """docstring for Drive"""
    local_url = "ipc:///tmp/drive.ipc"
    web_url = "ipc:///tmp/web.ipc"
    servo_url = "ipc:///tmp/servo.ipc"

    remote_urls = [web_url,servo_url]

    LEFT_PIN_1 = 6
    LEFT_PIN_2 = 13
    RIGHT_PIN_1 = 19
    RIGHT_PIN_2 = 26

    PWM_HZ = 125

    DEVICE_NAME = "drive"

    def __init__(self):
        super(Drive, self).__init__()

        self.OPERAT_DICT = {
            "move_stop" : self.move_stop,
            "move_forward": self.move_forward,
            "move_backward": self.move_backward,
            "move_left_bycenter": self.move_left_bycenter,
            "move_right_bycenter": self.move_right_bycenter,
        }

        self.ipc_sock = nnpy.Socket(nnpy.AF_SP, nnpy.BUS)
        self.ipc_sock.bind(self.local_url)

        self.gpio_init()


    def gpio_init(self):
        #GPIO.cleanup([self.LEFT_PIN_1, self.LEFT_PIN_2, self.RIGHT_PIN_1, self.RIGHT_PIN_2])
        GPIO.setmode(GPIO.BCM)
        GPIO.setup([self.LEFT_PIN_1, self.LEFT_PIN_2, self.RIGHT_PIN_1, self.RIGHT_PIN_2], \
                    GPIO.OUT)

        self.left_1 = GPIO.PWM(self.LEFT_PIN_1, self.PWM_HZ)
        self.left_2 = GPIO.PWM(self.LEFT_PIN_1, self.PWM_HZ)
        self.right_1 = GPIO.PWM(self.RIGHT_PIN_1, self.PWM_HZ)
        self.right_2 = GPIO.PWM(self.RIGHT_PIN_2, self.PWM_HZ)

        self.left_1.start(0)
        self.left_2.start(0)
        self.right_1.start(0)
        self.right_2.start(0)


    def msg_proc(self):
        while True:
            try:
                msg_bytes = self.ipc_sock.recv()
                msg_str = msg_bytes.decode('utf-8')
                msg = json.loads(msg_str)

                print(type(msg), msg)
                print(type(msg["head"]["device"]), msg["head"]["device"])
                if msg["head"]["device"] == self.DEVICE_NAME:
                    self.msg_parse(msg)
            except nnpy.errors.NNError as e:
                print(e)
                break

    def msg_parse(self, msg):
        method = msg["head"]["method"]
        if method in self.OPERAT_DICT:
            self.OPERAT_DICT[method]()

        self.ipc_sock.send("reply");

    def move_forward(self):
        print("move_forward")

        self._left_wheel_f()
        self.right_wheel_f()

    def move_backward(self):
        print("move_backward")
        self.left_wheel_b()
        self.right_wheel_b()

    def move_left_bycenter(self):
        print("move_left_bycenter")
        self.left_wheel_b()
        self.right_wheel_f()

    def move_right_bycenter(self):
        self._left_wheel_f()
        self.right_wheel_b()

    def move_stop(self):
        print("move_stop")

        self.left_wheel_stop()
        self.right_wheel_stop()

    def _left_wheel_f(self):
        self.left_1.ChangeDutyCycle(0)
        self.left_2.ChangeDutyCycle(90)

    def left_wheel_b(self):
        self.left_2.ChangeDutyCycle(0)
        self.left_1.ChangeDutyCycle(90)

    def left_wheel_stop(self):
        self.left_2.ChangeDutyCycle(0)
        self.left_1.ChangeDutyCycle(0)

    def right_wheel_f(self):
        self.right_1.ChangeDutyCycle(0)
        self.right_2.ChangeDutyCycle(90)

    def right_wheel_b(self):
        self.right_2.ChangeDutyCycle(0)
        self.right_1.ChangeDutyCycle(90)

    def right_wheel_stop(self):
        self.right_2.ChangeDutyCycle(0)
        self.right_1.ChangeDutyCycle(0)

    def __del__(self):
        self.left_1.stop()
        self.left_2.stop()
        self.right_1.stop()
        self.right_2.stop()
        GPIO.cleanup([self.LEFT_PIN_1, self.LEFT_PIN_2, self.RIGHT_PIN_1, self.RIGHT_PIN_2])

def main():
    drive = Drive()
    drive.msg_proc()
    print("exit!!!")
    
    time.sleep(5)

if __name__ == '__main__':
    main()
